#include "svpwm_orthogonal.hpp"
#include "math.h"

SVPWM_Orthogonal::Result SVPWM_Orthogonal::calculate(double alpha,double beta){
    Result result = {
        .p_alpha_1 = 0.0,
        .p_alpha_2 = 0.0,
        .p_beta_1 = 0.0,
        .p_beta_2 = 0.0
    };

    char code[4] = {
        0b0101,0b0110,0b1010,0b1001
    };

    /* 计算模长. */
    double m = sqrt(alpha * alpha + beta * beta);
    if ( m > 1 ){
        alpha = alpha / m;
        beta = beta / m;
    }
    int sector = 0;
    /* 计算扇区. */
    if ( alpha > 0 ){
        if ( beta > 0 ){
            sector = 0;
        }else{
            sector = 3;
        }
    }else{
        if ( beta > 0 ){
            sector = 1;
        }else{
            sector = 2;
        }
    }

    if ( alpha < 0 ){
        alpha = -alpha;
    }
    if ( beta < 0 ){
        beta = -beta;
    }

    /* 计算alpha与beta的关闭占空比. */
    double p_alpha_closed = 1 - alpha;
    double p_beta_closed = 1 - beta;
    
    result.p_alpha_1 = alpha * (code[sector] & 0x01) + p_alpha_closed / 2;
    result.p_alpha_2 = alpha * ((code[sector] >> 1) & 0x01) + p_alpha_closed / 2;
    result.p_beta_1 = beta * ((code[sector] >> 2) & 0x01) + p_beta_closed / 2;
    result.p_beta_2 = beta * ((code[sector] >> 3) & 0x01) + p_beta_closed / 2;

    return result;
}
